#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <cmath>  // 补充数学库头文件
#include <algorithm>

class IMUController : public rclcpp::Node {
public:
    IMUController() : Node("imu_controller_node") {
        // 初始化发布器（避免全局变量问题）
        vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1000);
        
        // 初始化订阅器
        imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
            "imu/data", 1000, std::bind(&IMUController::IMUCallBack, this, std::placeholders::_1)
        );
        
        RCLCPP_INFO(this->get_logger(), "IMU Controller Node is running...");
    }

private:
    void IMUCallBack(const sensor_msgs::msg::Imu::SharedPtr msg) {
        // 提取IMU的四元数姿态
        tf2::Quaternion q(
            msg->orientation.x,
            msg->orientation.y,
            msg->orientation.z,
            msg->orientation.w
        );

        // 转换为欧拉角（滚转、俯仰、航向）
        double roll, pitch, yaw;
        tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
        
        // 转换为角度（°）
        roll *= 180.0 / M_PI;
        pitch *= 180.0 / M_PI;
        yaw *= 180.0 / M_PI;

        RCLCPP_INFO(this->get_logger(), "Roll: %.2f, Pitch: %.2f, Yaw: %.2f", roll, pitch, yaw);

        // 目标航向角（90°）
        const double target_yaw = 90.0;
        
        // 计算角度差（处理[-180, 180]范围的边界问题）
        double diff_angle = target_yaw - yaw;
        // 将角度差归一化到[-180, 180]
        if (diff_angle > 180.0) {
            diff_angle -= 360.0;
        } else if (diff_angle < -180.0) {
            diff_angle += 360.0;
        }

        // 发布速度指令
        geometry_msgs::msg::Twist vel_msg;
        
        // 比例控制计算角速度（限制最大角速度为1.0 rad/s）
        vel_msg.angular.z = diff_angle * 0.01;
        //vel_msg.angular.z = std::clamp(vel_msg.angular.z, -1.0, 1.0);  // 限制范围
        
        // 接近目标角度时再前进（例如角度差小于5°）
        if (std::abs(diff_angle) < 5.0) {
            vel_msg.linear.x = 0.1;  // 前进
        } else {
            vel_msg.linear.x = 0.0;  // 未对准目标时停止前进
        }
        
        vel_pub_->publish(vel_msg);
    }

    // 成员变量（替代全局变量）
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
};

int main(int argc, char**argv) {
    rclcpp::init(argc, argv);
    // 使用类实例管理节点生命周期（避免全局变量）
    rclcpp::spin(std::make_shared<IMUController>());
    rclcpp::shutdown();
    return 0;
}
